/*
 * Wavelet.c
 *
 */


#include "Wavelet.h"
#include "globe_var.h"
#include <math.h>

#define DPOINT_64	64
#define DPOINT_32	32
#define DPOINT_16	16
#define DPOINT_8	8

#define h0		(1+sqrt(3))/(4*sqrt(2))
#define h1		(3+sqrt(3))/(4*sqrt(2))
#define h2		(3-sqrt(3))/(4*sqrt(2))
#define h3		(1-sqrt(3))/(4*sqrt(2))
#define g0		h3
#define g1		-h2
#define g2		h1
#define g3		-h0
#define Ih0		h2
#define Ih1		g2
#define Ih2		h0
#define Ih3		g0
#define Ig0		h3
#define Ig1		g3
#define Ig2		h1
#define Ig3		g1

static double s1[DPOINT_64] = {0};
static double d1[DPOINT_64] = {0};
static double s2[DPOINT_16] = {0};
static double d2[DPOINT_16] = {0};
static double s3[DPOINT_8] = {0};
static double d3[DPOINT_8] = {0};

void Wavelet_D_64(double *src, double data, unsigned char idx, unsigned char num)
{
	if(num < DPOINT_64)
	{
		src[idx] = data;
	}
	else
	{
		int i;
		int j;
		int delta = 0;
		
		src[idx] = data;
		
		//step 1:
		for(j = 1; j < DPOINT_64 - 2; j+=2)
		{
			s1[i] = (src[j-1]*h0) + (src[j]*h1) + (src[j+1]*h2) + (src[j+2]*h3);
			d1[i] = (src[j-1]*g0) + (src[j]*g1) + (src[j+1]*g2) + (src[j+2]*g3);
			i++;
		}
		s1[i] = (src[62]*h0) + (src[63]*h1) + (src[0]*h2) + (src[1]*h3);
		d1[i] = (src[62]*g0) + (src[63]*g1) + (src[0]*g2) + (src[1]*g3);
		
		for(i = 0; i < DPOINT_32; i++)
		{
			if(fabs(d1[i]) < delta)
				d1[i] = 0;
			else if(d1[i] < 0)
				d1[i] += delta;
			else
				d1[i] -= delta;
			d1[i] = 0;
		}
		
		//step 2:
		i = 0;
		for(j = 1; j < DPOINT_32-2; j+=2)
		{
			s2[i] = (s1[j-1]*h0) + (s1[j]*h1) + (s1[j+1]*h2) + (s1[j+2]*h3);
			d2[i] = (d1[j-1]*g0) + (d1[j]*g1) + (d1[j+1]*g2) + (d1[j+2]*g3);
			i++;
		}
		s2[i] = (s1[30]*h0) + (s1[31]*h1) + (s1[0]*h2) + (s1[1]*h3);
		d2[i] = (d1[30]*g0) + (d1[31]*g1) + (d1[0]*g2) + (d1[1]*g3);
		
		delta = 0;
		for(i = 0; i < DPOINT_16; i++)
		{
			if(fabs(d2[i]) < delta)
				d2[i] = 0;
			else if(d2[i] < 0)
				d2[i] += delta;
			else
				d2[i] -= delta;
			d2[i] = 0;
		}
		
		//step 3:
		i = 0;
		for(j = 0; j < DPOINT_16-2; j+=2)
		{
			s3[i] = (s2[j-1]*h0) + (s2[j]*h1) + (s2[j+1]*h2) + (s2[j+2]*h3);
			d3[i] = (d2[j-1]*g0) + (d2[j]*g1) + (d2[j+1]*g2) + (d2[j+2]*g3);
			i++;
		}
		s3[i] = (s2[14]*h0) + (s2[15]*h1) + (s2[0]*h2) + (s2[1]*h3);
		d3[i] = (d2[14]*g0) + (d2[15]*g1) + (d2[0]*g2) + (d2[1]*g3);
		
		delta = 0;
		for(i = 0; i < DPOINT_8; i++)
		{
			if(fabs(d3[i]) < delta)
				d3[i] = 0;
			else if(d3[i] < 0)
				d3[i] += delta;
			else
				d3[i] -= delta;
			d3[i] = 0;
		}
		
		//step 4:
		s2[0] = (s3[DPOINT_8-1]*Ih0) + (d3[DPOINT_8-1]*Ih1) + (s3[0]*Ih2) + (d3[0]*Ih3);
		s2[1] = (s3[DPOINT_8-1]*Ig0) + (d3[DPOINT_8-1]*Ig1) + (s3[0]*Ig2) + (d3[0]*Ig3);
		for(i = 1; i < DPOINT_8; i++)
		{
			s2[2*i] = (s3[i-1]*Ih0) + (d3[i-1]*Ih1) + (s3[i]*Ih2) + (d3[i]*Ih3);
			s2[(2*i)+1] = (s3[i-1]*Ig0) + (d3[i-1]*Ig1) + (s3[i]*Ig2) + (d3[i]*Ig3);
		}
		
		//step 5:
		s1[0] = (s2[DPOINT_16-1]*Ih0) + (d2[DPOINT_16-1]*Ih1) + (s2[0]*Ih2) + (d2[0]*Ih3);
		s1[1] = (s2[DPOINT_16-1]*Ig0) + (d2[DPOINT_16-1]*Ig1) + (s2[0]*Ig2) + (d2[0]*Ig3);
		for(i = 1; i < DPOINT_16; i++)
		{
			s1[2*i] = (s2[i-1]*Ih0) + (d2[i-1]*Ih1) + (s2[i]*Ih2) + (d2[i]*Ih3);
			s1[(2*i)+1] = (s2[i-1]*Ig0) + (d2[i-1]*Ig1) + (s2[i]*Ig2) + (d2[i]*Ig3);
		}
		
		//step 6:
		src[0] = (s1[DPOINT_32-1]*Ih0) + (d1[DPOINT_32-1]*Ih1) + (s1[0]*Ih2)+ (s1[0]*Ih3);
		src[1] = (s1[DPOINT_32-1]*Ig0) + (d1[DPOINT_32-1]*Ig1) + (s1[0]*Ig2)+ (s1[0]*Ig3);
		for(i = 1; i < DPOINT_32; i++)
		{
			src[2*i] = (s1[i-1]*Ih0) + (d1[i-1]*Ih1) + (s1[0]*Ih2)+ (s1[0]*Ih3);
			src[(2*i)+1] = (s1[i-1]*Ig0) + (d1[i-1]*Ig1) + (s1[0]*Ig2)+ (s1[0]*Ig3);
		}
	}
}


/** @} */